#include "SensorModel.h"

SensorModel::SensorModel(ros::NodeHandle n) : laser(n) {
	cost_marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 10);
}

SensorModel::~SensorModel() {
}

void SensorModel::setObstacleCostMap(const MATRIX &m){
	obstacleCostMap = m;
}

void SensorModel::estimateWeights(const PoseArray &pa, const Robot &robot, WeightsForPoses &w){
	// first, get the most-recent laser-scan
	//ROS_INFO("Sensor Model");
	SensorMessages sr = laser.scan();
	float maxRange = sr->range_max;
	float minRange = sr->range_min;
	float angleMin = sr->angle_min, angleMax = sr->angle_max, angleIncrement = sr->angle_increment;
	double maxprob = -INF;

	// fetch robot error-characteristics
	double gaussianVariance	= atof(robot.getConfiguration("robot.sensor.phit.variance").c_str());
	int laserAngleStep 		= atoi(robot.getConfiguration("robot.sensor.skipAngle").c_str());
	double prand 			= atof(robot.getConfiguration("robot.sensor.prand").c_str());
	double zhit 			= atof(robot.getConfiguration("robot.sensor.zhit").c_str());
	double zrand 			= atof(robot.getConfiguration("robot.sensor.zrand").c_str());

	double epsilon = 0.01;
	//cout <<  "Variance " << gaussianVariance << ", laserAngleStep " << laserAngleStep << endl;

	VECTOR poseProbability(pa.size(), 1.0);
	double sumPoseProbability = 0;
	// foreach pose
	for (int i = 0; i < pa.size(); ++i){
    	MyPose odomPose = pa[i];

		// get pose in map-coords
		double mapX, mapY, mapTheta;
		odomPose.getPoseInMap(mapX, mapY, mapTheta);
		PII baseInMap = index(mapX,mapY,CELL_RESOLUTION);
		// cout <<  "Pose in map: " << mapX << ", " << mapY << ", " << mapTheta << endl;

		// foreach laser-ray
		for (float angle = angleMin; angle <= angleMax; angle += angleIncrement*laserAngleStep)	{

			int rangeIndex = (angle - angleMin) / angleIncrement;
			float distance = sr->ranges[rangeIndex];

			// check for spurious laser-readings
			if (distance < minRange || distance > maxRange)	{
				continue;
			}

			// ignore all laser-readings which are at maxRange
			if (fabs(distance - maxRange) < epsilon){
				continue;
			}

			// determine end of laser-ray
			// end-of-laser-ray = current-robot-point + laser-ray
			double laserX = mapX + distance*cos(mapTheta + angle);
			double laserY = mapY + distance*sin(mapTheta + angle);

			// find closest obstacle to that point in the map
			PII laserInMap = index(laserX,laserY,CELL_RESOLUTION);

			/** Laser has shot out of the map if the following conditions are true **/
			if(laserInMap.first >= obstacleCostMap.size() || laserInMap.second >= obstacleCostMap[0].size())
				continue;
			double closestObstacleDistance = obstacleCostMap[laserInMap.first][laserInMap.second];

			// determine laser-match to that point
			double probability = 0.0, hitprob,  missprob;
			hitprob = zhit * gaussianProbability(distance, gaussianVariance, distance-closestObstacleDistance);
			missprob = zrand * prand;
			//ROS_INFO("%lf %lf",hitprob,missprob);
			probability = hitprob + missprob;

		/**
		 *if(angle < -80.0f*PI/180.0f){
		 * 	cout <<  "Laser-point angle: " << angle*180/PI << ", point" << laserInMap.first << " " << laserInMap.second << endl;
		 * 	cm.PlotPoses(laserInMap.first,laserInMap.second,cost_marker_pub,CELL_RESOLUTION);
		 * 	std::cout <<  "closestO " << closestObstacleDistance << std::endl;
		 * 	cout <<  "prob " << gaussianProbability(distance, gaussianVariance, distance-closestObstacleDistance) << endl;
		 * 	cout << closestObstacleDistance << " " << distance << " " << gaussianVariance << std::endl;
		 * }
		 */
			// assumes independence of laser-rays
			// and multiplies probabilities.
			poseProbability[i] = poseProbability[i] * probability;

		} // end laser-angle

		sumPoseProbability += poseProbability[i];
		if(poseProbability[i] > maxprob){
			maxprob = poseProbability[i];
		}

	} // end poses

	//std::cout << maxprob << " " << sumPoseProbability << " | ";
	// normalize
	for (int i = 0; i < poseProbability.size() && poseProbability.size() > 1; ++i)	{
		poseProbability[i] /= sumPoseProbability;
	}

	w = poseProbability;

}

double SensorModel::gaussianProbability(double mean, double variance, double x)	{

	return (1 / sqrt(2 * PI * variance)) * exp( -(x-mean)*(x-mean) / (2*variance) );

}
